/*----------------------------------------------------------------------------*/
/* Team 3647
 /* DriveTrain.java
 /* Drive functions and compressor 
 /*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Talon;

public class Drivetrain {

    static Talon LeftTalon = new Talon(1);
    static Talon RightTalon = new Talon(2);
    public static Compressor comp = new Compressor(1, 1);

    /**
     * *************************************************************************,
     * Drive function based off two joysticks Uses the Cheesy Drive function.
     * *************************************************************************
     */
    public static void TankCD() {
        double throttle = (JoySticks.LeftJoyStick + JoySticks.RightJoyStick) / 2;
        double wheel    = (JoySticks.LeftJoyStick - JoySticks.RightJoyStick) / 2;

        if (Math.abs(throttle) <= 0.3 && Math.abs(throttle) >= 0.05) {
            throttle = 0.3 * (throttle < 0 ? -1 : 1);
        } else if (JoySticks.driveForward) {
            throttle = .4;
            wheel    = 0;
        } else if (JoySticks.driveBackwards) {
            throttle = -.4;
            wheel    = -0; 
        } 
        CheesyDrive(throttle, wheel, true);
        //chessyDrivev2(throttle, wheel);
    }
    
    private static double old_wheel = 0.0;
    private static double neg_inertia_accumulator = 0.0;
    public static void chessyDrivev2(double wheel, double throttle) {
        double left_pwm, right_pwm, overPower;
        double sensitivity = 1.2;
        double angular_power;
        double linear_power;
        double wheelNonLinearity;
        boolean quickTurn = Math.abs(throttle) < .05;//Math.abs(wheel) > .375 &&	..Notic


        double  neg_inertia = wheel - old_wheel;
        old_wheel = wheel;

        wheelNonLinearity = 0.9; // used to be csvReader->TURN_NONL
        // Apply a sin function that's scaled to make it feel better
        wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / Math.sin(Math.PI / 2.0 * wheelNonLinearity);
        wheel = Math.sin(Math.PI / 2.0 * wheelNonLinearity * wheel) / Math.sin(Math.PI / 2.0 * wheelNonLinearity);

        double neg_inertia_scalar;

        neg_inertia_scalar = 3; 
        sensitivity = 1.15; 

        neg_inertia_scalar *= .375; //from jeremy: I scaled down the negative inertia scalar because it was acting up at sac. tune this up higher  for tighter turn braking
        double neg_inertia_power = neg_inertia * neg_inertia_scalar;

        if (Math.abs(throttle) >= 0.05 || quickTurn) {
            neg_inertia_accumulator += neg_inertia_power;
        }
        wheel = wheel + neg_inertia_accumulator;
        if (neg_inertia_accumulator > 1) {
            neg_inertia_accumulator -= 1;
        } else if (neg_inertia_accumulator < -1) {
            neg_inertia_accumulator += 1;
        } else {
            neg_inertia_accumulator = 0;
        }

        linear_power = throttle;

        if ((!isInBand(throttle, -0.2, 0.2) || !(isInBand(wheel, -0.65, 0.65))) && quickTurn) {
            overPower = 1.0;
            sensitivity = 1.0;
            angular_power = wheel;
        } else {
            overPower = 0.0;
            angular_power = Math.abs(throttle) * wheel * sensitivity;
        }
        //System.out.println("NA " + neg_inertia_accumulator + " AP " + angular_power + " wheel " + wheel + " throttle" + throttle + " NAP " + neg_inertia_power);
        right_pwm = left_pwm = linear_power;
        left_pwm += angular_power;
        right_pwm -= angular_power;

        if (left_pwm > 1.0) {
            right_pwm -= overPower * (left_pwm - 1.0);
            left_pwm = 1.0;
        } else if (right_pwm > 1.0) {
            left_pwm -= overPower * (right_pwm - 1.0);
            right_pwm = 1.0;
        } else if (left_pwm < -1.0) {
            right_pwm += overPower * (-1.0 - left_pwm);
            left_pwm = -1.0;
        } else if (right_pwm < -1.0) {
            left_pwm += overPower * (-1.0 - right_pwm);
            right_pwm = -1.0;
        }
        Drive((left_pwm), (right_pwm));
        System.out.println(left_pwm + "    " + right_pwm);
    }
    
    public static boolean isInBand(double input, double low, double high) {
        return input > low && input < high;
    }
    

    public static void CheesyDrive(double throttle, double wheel, boolean quickTurn) {
        double angular_power = 0.0;
        double overPower = 0.0;
        double sensitivity = 1.5;
        double rPower = 0.0;
        double lPower = 0.0;
        if (quickTurn) {
            overPower = .25;
            sensitivity = .75;
            angular_power = wheel;
        } else {
            overPower = 0.0;
            angular_power = Math.abs(throttle) * wheel * sensitivity;
        }
        rPower = lPower = throttle;
        lPower += angular_power;
        rPower -= angular_power;
        if (lPower > 1.0) {
            rPower -= overPower * (lPower - 1.0);
            lPower = 1.0;
        } else if (rPower > 1.0) {
            lPower -= overPower * (rPower - 1.0);
            rPower = 1.0;
        } else if (lPower < -1.0) {
            rPower += overPower * (-1.0 - lPower);
            lPower = -1.0;
        } else if (rPower < -1.0) {
            lPower += overPower * (-1.0 - rPower);
            rPower = -1.0;
        }
        Drive(lPower, rPower);
    }

    public static void Drive(double left, double right) {
        LeftTalon.set(-left);
        RightTalon.set(right);
    }

    public static void runCompessor() {
        comp.start();
    }
}
